#include "linemod_detector_Tool.h"
#include "opencv2/core.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "CsvData.h"
#include <ctime>
#include <iostream>
#include <pcl/visualization/pcl_visualizer.h>
#include "PclViewTools.h"
#include "opencv2/rgbd.hpp"
#include <pcl/PolygonMesh.h>
#include "Eigen/Core"
using namespace std;
using namespace cv;
#define image_width 960
#define image_high 540
#define PCL
// #define CsvData_tool
Eigen::Vector3f CubeTranslation;
Eigen::Quaternionf CubeRotation;
std::vector<pose> outposedd;
std::vector<Eigen::Vector3f> CubeTranslationV;
std::vector<Eigen::Quaternionf> CubeRotationV;
cv::Mat colorImg;

#define PI (3.1415926535897932346f)
using namespace pcl;
#pragma region toString
template <class T>
string toString(const T &v)
{
  ostringstream os;
  os << v;
  return os.str();
}
#pragma endregion

#pragma region viewerOneOff
void viewerOneOff(pcl::visualization::PCLVisualizer &viewer)
{

  if (CubeTranslationV.size() != 0)
  {
    // for( int i = 0; i < outposedd.size() ; i++){
    std::cout << "add cube" << std::endl;

    for (int i = 0; i < 1; i++)
    {
      std::string cubeName = "cube " + toString(i);
      viewer.removeAllShapes();
      viewer.addCube(CubeTranslationV[i], CubeRotationV[i], 0.2, 0.1, 0.05, cubeName);
      // viewer.addCylinder
    }
  }

  std::cout << "I only run once" << std::endl;
}


void viewerRunnerCloudViewer(pcl::visualization::CloudViewer *viewer)
{
  while (!viewer->wasStopped())
  {
    //  viewer->spinOnce(100);
    boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  }
}

#pragma endregion

#pragma region onMouse
cv::Point pt;
void onMouse(int event, int x, int y, int flag, void *)
{

  switch (event)
  {
  case CV_EVENT_LBUTTONDOWN: //左键按下
    flag = true;
    pt.x = x;
    pt.y = y;
    break;
  case CV_EVENT_MOUSEMOVE: //鼠标移动
    if (flag)
    { //只有在点击鼠标左键的情况下移动鼠标才执行
      // img_copy.copyTo(img);
      //以便永远只有一个矩形（否则许多矩形会重叠在一起），清除之前在img图像上，鼠标移动产生的矩形框，只保留最终的一个矩形框
      // rectangle(img, pt, Point(x, y), Scalar(255, 0, 0), 2, 8);//定义一个矩形框，pt表示起始点（按下左键时的鼠标左标）；Point表示实时变化的坐标点；颜色；线宽；线型
    }
    break;
  case CV_EVENT_LBUTTONUP: //左键抬起
    flag = false;
    // ROI = img_copy(Rect(pt.x, pt.y, x - pt.x, y - pt.y));//x,y,宽，高
    // imshow("ROI", ROI);
    // imwrite("filename",ROI);
    break;
  default:
    break;
  }
}
#pragma endregion

//typedef pcl::PointCloud<PointT> PointCloud;

int main()
{
  // std::string rootFolder = "/home/de/linemod测试样本/data-test/";
  std::string rootFolder = "/home/de/linemod测试样本/data-test/";
  vector<string> LogData;
#ifdef CsvData_tool
  CsvData *C_data = new CsvData();
  C_data->Init("demo.csv", {"image ", "output ", "x", "y", "z", "qx", "qy", "qz", "qw"});
#endif
  LinemodDetector *LineDect = new LinemodDetector();
  Mat xmlDepth, DepthShow;
#ifdef PCL
  PclViewTools *pclViewTool = new PclViewTools();

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  // pcl::io::loadPLYFile("/home/de/Downloads/icbin_bop19/models/obj_000001.ply", *pcloud); //加载点云文件
  pcl::visualization::CloudViewer *viewer = new pcl::visualization::CloudViewer("Simple Cloud Viewer");

  // viewer->runOnVisualizationThreadOnce(*viewerOneOff);
  CubeTranslationV.push_back(Eigen::Vector3f(0,0,0));
  CubeRotationV.push_back(Eigen::Quaternionf(1,0,0,0));
  //
  boost::thread vthread(&viewerRunnerCloudViewer,viewer);
  /******************************************************** */

#endif
  LineDect->loadData("/home/de/hirop_data/milk13", "milk", "ply");
  // LineDect->loadData("/home/de/hirop_data/milk_big","milk","ply");
  // LineDect->loadData("/home/de/hirop_data/milk_stl","milk","stl");
  // LineDect->loadData("/home/de/hirop_data/milk_bottem","milk","ply");
  // LineDect->loadData("/home/de/hirop_data/milk_test","milk","ply");

  // int testNum = 3;
  int id = 0;

  for (int i = 1; i < 150; i++)
  {

    // LogData.clear();
    // LogData.reserve(9);
    
    // colorImg = cv::Mat::zeros(Size(960,540), CV_8UC3);
    // colorImg = cv::imread("/home/de/build-20190716/1/ColorImg.png");
    // resize(colorImg,colorImg,Size(960,540));
    //   DepthShow = cv::imread(rootFolder+"depth/show/"+to_string(i)+".jpg",IMREAD_GRAYSCALE);
    //   DepthShow.convertTo(DepthShow, CV_16UC1, 1000.0);

    //输入图像
    cout << "********************************** " << i << endl;

    colorImg = cv::imread(rootFolder + "JPEGImages/ColorZero" + to_string(i) + ".png");
    FileStorage fs(rootFolder + "depth/depthTest" + to_string(i) + ".xml", FileStorage::READ);
    cv::Mat matGray,matThresh ;
    // cv::split(colorImg, blue);
    // cvtColor(colorImg, matGray, CV_BGR2GRAY);
    // // threshold(matGray, matThresh, 156, 255, CV_THRESH_TOZERO_INV);
    // adaptiveThreshold(matGray, matThresh, 255, ADAPTIVE_THRESH_GAUSSIAN_C, THRESH_BINARY_INV, 5, 5);
    // imshow("gray", matGray);
    // imshow("thresh", matThresh);
    // cv::waitKey(0);
    fs["depth"] >> xmlDepth;
    fs.release();
#pragma region //测试可视化是否正常
               //测试可视化是否正常
               // cv::Mat ColorTemp = cv::Mat::zeros(Size(960,540), CV_8UC3);
               // cv::Mat DepthTemp = cv::Mat::zeros(Size(960,540), CV_16UC1);
               // Rect  rRoi = Rect(ColorTemp.cols/2, ColorTemp.rows/2, colorImg.cols, colorImg.rows);
               // cv::Mat ColorTempR =  ColorTemp(rRoi);
               // cv::Mat DepthTempR =  DepthTemp(rRoi);
               // colorImg.copyTo(ColorTempR);
               // xmlDepth.copyTo(DepthTempR);
               // xmlDepth = DepthTemp;
               // colorImg = ColorTemp;

    // imshow("ColorTemp", ColorTemp);
    // imshow("DepthTemp",  DepthTemp);
#pragma endregion

#pragma region //ROI裁剪
    // cv::Mat ColorTemp = cv::Mat::zeros(Size(960,540), CV_8UC3);
    // cv::Mat DepthTemp = cv::Mat::zeros(Size(960,540), CV_16UC1);
    // Rect  rRoi = Rect(Point(418,218), Point(487,300));
    // cv::Mat ColorTempR =  ColorTemp(rRoi);
    // cv::Mat DepthTempR =  DepthTemp(rRoi);

    // cv::Mat ColorR =  colorImg(rRoi);
    // cv::Mat DepthR =  xmlDepth(rRoi);

    // ColorR.copyTo(ColorTempR);
    // DepthR.copyTo(DepthTempR);

    // xmlDepth = DepthTemp;
    // colorImg = ColorTemp;
#pragma endregion

    // imshow("DepthImg1", xmlDepth);
    imshow("colorImg", colorImg);
    imshow("xmlDepth", xmlDepth);
    cv::waitKey(10);
    // cv::waitKey(0);


    LineDect->setDepthImg(xmlDepth);
    LineDect->setColorImg(colorImg);
    int rtn1 = LineDect->detection();
    if (rtn1 == 0)
    {
      LineDect->getResult(outposedd);

#pragma region //C_data->WriteData
#ifdef CsvData_tool
      LogData.push_back(to_string(i));
      LogData.push_back(to_string(outposedd.size()));
      LogData.push_back(to_string(outposedd[0].position.x));
      LogData.push_back(to_string(outposedd[0].position.y));
      LogData.push_back(to_string(outposedd[0].position.z));
      LogData.push_back(to_string(outposedd[0].quaternion.x));
      LogData.push_back(to_string(outposedd[0].quaternion.y));
      LogData.push_back(to_string(outposedd[0].quaternion.z));
      LogData.push_back(to_string(outposedd[0].quaternion.w));
      C_data->WriteData(LogData);
#endif
#pragma endregion
      cout << " find! " << endl;
    }
    else
    {
      cout << "non find " << endl;
      perror("bad");
  
    }
#ifdef PCL

    pclViewTool->DepthToCloud(colorImg, xmlDepth, pcloud);

    CubeTranslationV.clear();
    CubeRotationV.clear();
    for (int i = 0; i < outposedd.size(); i++)
    {
      CubeTranslation = Eigen::Vector3f();
      CubeTranslation << outposedd[i].position.x, outposedd[i].position.y, outposedd[i].position.z;
      float changeQuater[4];
      changeQuater[0] = static_cast<float>(outposedd[i].quaternion.x);
      changeQuater[1] = static_cast<float>(outposedd[i].quaternion.y);
      changeQuater[2] = static_cast<float>(outposedd[i].quaternion.z);
      changeQuater[3] = static_cast<float>(outposedd[i].quaternion.w);
      cout << " changeQuater " << changeQuater[0] << " " << changeQuater[1] << " " << changeQuater[2] << " " << changeQuater[3] << std::endl;
      // CubeTranslation<<0,0,0;
      CubeRotation = Eigen::Quaternionf(changeQuater[3], changeQuater[0], changeQuater[1],
                                        changeQuater[2]);
      std::cout << "CubeRotation x  " << CubeRotation.x() << std::endl;
      std::cout << "CubeRotation y  " << CubeRotation.y() << std::endl;
      std::cout << "CubeRotation z  " << CubeRotation.z() << std::endl;
      std::cout << "CubeRotation w  " << CubeRotation.w() << std::endl;

      Eigen::Quaterniond CubeRotationShow = Eigen::Quaterniond(outposedd[i].quaternion.w, outposedd[i].quaternion.x,
                                                               outposedd[i].quaternion.y, outposedd[i].quaternion.z);
      Eigen::Vector3d eulerAngle = CubeRotationShow.matrix().eulerAngles(2, 1, 0);
      std::cout << "输出 Z-Y-X，即RPY  " << eulerAngle / PI * 180 << std::endl
                << endl;

      // 测试代码
      // 刷新 cube 的姿态
      CubeTranslationV.push_back(CubeTranslation);
      CubeRotationV.push_back(CubeRotation);

    }

    if( outposedd.size() == 0){
        CubeTranslationV.push_back(Eigen::Vector3f(0,0,0));
        CubeRotationV.push_back(Eigen::Quaternionf(1,0,0,0));
    }

    cout << "continue pcloud ... " << endl;

    //只渲染一次
    viewer->runOnVisualizationThreadOnce(*viewerOneOff);
    viewer->showCloud(pcloud);

    cv::waitKey(0);

    outposedd.clear();

#endif
  }
#ifdef CsvData_tool
  C_data->close();
  delete C_data;
#endif
#ifdef PCL
  //delete viewer;

  delete pclViewTool;
#endif
  delete LineDect;
  return 0;
}

